#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static double _point_polygon_test(const std::vector<cv::Point>& contour, cv::Point2f pt, bool measureDist) {
    int total = contour.size();
    CV_Assert(total >= 0);
    
    if (total == 0) {
        return (measureDist ? -DBL_MAX : -1);
    }
    
    double result = 0;
    int count = 0;
    if (!measureDist) {
        cv::Point p(cvRound(pt.x), cvRound(pt.y));
        if (p.x == pt.x && p.y == pt.y) {
            cv::Point a, b = contour[total-1];
            for (int i = 0; i < total; i++) {
                a = b;
                b = contour[i];
                
                if ((a.y <= p.y && b.y <= p.y)
                    || (a.y > p.y && b.y > p.y)
                    || (a.x < p.x && b.x < p.x)) {
                    if (p.y == b.y && (p.x == b.x
                        || (p.y == a.y &&
                            ((a.x <= p.x && p.x <= b.x)
                            || (b.x <= p.x && p.x <= a.x))))) {
                        return 0;
                    }
                    continue;
                }
                int64 dist = static_cast<int64>(p.y - a.y) * (b.x - a.x) - static_cast<int64>(p.x - a.x) * (b.y - a.y);
                if (dist == 0) {
                    return 0;
                }
                if (b.y < a.y) {
                    dist = -dist;
                }
                if (dist > 0) {
                    count++;
                }
            }
        } else {
            cv::Point2f a, b = cv::Point2f(contour[total-1].x, contour[total-1].y);
            for (int i = 0; i < total; i++) {
                a = b;
                b = cv::Point2f(contour[i].x, contour[i].y);
                
                if ((a.y <= pt.y && b.y <= pt.y)
                    || (a.y > pt.y && b.y > pt.y)
                    || (a.x < pt.x && b.x < pt.x)) {
                    if (pt.y == b.y && (pt.x == b.x
                        || (pt.y == a.y &&
                            ((a.x <= pt.x && pt.x <= b.x)
                            || (b.x <= pt.x && pt.x <= a.x))))) {
                        return 0;
                    }
                    continue;
                }
                double dist = (double)(pt.y - a.y) * (b.x - a.x) - (double)(pt.x - a.x) * (b.y - a.y);
                if (dist == 0) {
                    return 0;
                }
                if (b.y < a.y) {
                    dist = -dist;
                }
                if (dist > 0) {
                    count++;
                }
            }
        }
        result = (count % 2 == 0 ? -1 : 1);
    } else {
        double min_dist_num = FLT_MAX, min_dist_denom = 1;
        cv::Point2f a, b = cv::Point2f(contour[total-1].x, contour[total-1].y);
        for (int i = 0; i < total; i++) {
            a = b;
            b = cv::Point2f(contour[i].x, contour[i].y);
            double dx_ba = b.x - a.x, dy_ba = b.y - a.y;
            double dx_pa = pt.x - a.x, dy_pa = pt.y - a.y;
            double dx_pb = pt.x - b.x, dy_pb = pt.y - b.y;
            
            double dist_num, dist_denom = 1;
            if (dx_ba * dx_pa + dy_ba * dy_pa <= 0) {
                dist_num = dx_pa * dx_pa + dy_pa * dy_pa;
            } else if (dx_ba * dx_pb + dy_ba * dy_pb >= 0) {
                dist_num = dx_pb * dx_pb + dy_pb * dy_pb;
            } else {
                dist_num = dx_ba * dy_pa - dy_ba * dx_pa;
                dist_num *= dist_num;
                dist_denom = dx_ba * dx_ba + dy_ba * dy_ba;
            }
            
            if(dist_num * min_dist_denom < min_dist_num * dist_denom) {
                min_dist_num = dist_num;
                min_dist_denom = dist_denom;
                if (min_dist_num == 0) {
                    break;
                }
            }
            
            if ((a.y <= pt.y && b.y <= pt.y)
                    || (a.y > pt.y && b.y > pt.y)
                    || (a.x < pt.x && b.x < pt.x)) {
                continue;
            }
            double dist = dy_pa * dx_ba - dx_pa * dy_ba;
            if (dy_ba < 0) {
                dist = -dist;
            }
            if (dist > 0) {
                count++;
            }
        }
        result = std::sqrt(min_dist_num / min_dist_denom);
        if (count % 2 == 0) {
            result = -result;
        }
    }
    return result;
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/templ.png", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    cv::copyMakeBorder(src, src, 100, 100, 100, 100, cv::BORDER_CONSTANT, cv::Scalar::all(255));
    
    cv::Mat gray;
    cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
    cv::Mat binary;
    cv::threshold(gray, binary, 128, 255, cv::THRESH_BINARY_INV);
    
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(binary, contours, hierarchy, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
    CV_Assert(contours.size() == 1);
    
    std::vector<cv::Point2f> pts;
    pts.push_back(cv::Point2f(50, 50));
    pts.push_back(cv::Point2f(src.cols/2, src.rows /2));
    pts.push_back(contours[0][0]);
    std::vector<cv::Scalar> colors;
    colors.push_back(cv::Scalar(0, 0, 255));
    colors.push_back(cv::Scalar(0, 255, 0));
    colors.push_back(cv::Scalar(255, 0, 0));
    for (int i = 0; i < pts.size(); i++) {
        //double dist = cv::pointPolygonTest(contours[0], pts[i], false);
        double dist = cv::pointPolygonTest(contours[0], pts[i], true);
        std::cout << "point " << i << ": dist = " << dist << ", " << (dist > 0 ? "inside contour" : (dist < 0 ? "outside contour" : "on contour edge")) << std::endl;
        cv::circle(src, pts[i], 3, colors[i], cv::FILLED);
        
        //dist = _point_polygon_test(contours[0], pts[i], false);
        dist = _point_polygon_test(contours[0], pts[i], true);
        std::cout << "_point_polygon_test: " << std::endl;
        std::cout << "point " << i << ": dist = " << dist << ", " << (dist > 0 ? "inside contour" : (dist < 0 ? "outside contour" : "on contour edge")) << std::endl;
        std::cout << "------------------------------------------------" << std::endl;
    }
    
    cv::Mat distMap = cv::Mat::zeros(src.size(), CV_64FC1);
    for (int i = 0; i < src.rows; i++) {
        for (int j = 0; j < src.cols; j++) {
            cv::Point2f pt(j, i);
            double dist = cv::pointPolygonTest(contours[0], pt, true);
            distMap.ptr<double>(i)[j] = dist;
        }
    }
    double minVal, maxVal;
    cv::minMaxLoc(distMap, &minVal, &maxVal);
    cv::Mat distMap2 = cv::Mat::zeros(src.size(), CV_8UC3);
    for (int i = 0; i < src.rows; i++) {
        for (int j = 0; j < src.cols; j++) {
            double mapVal = distMap.ptr<double>(i)[j];
            cv::Vec3b c(0, 0, 0);
            if (mapVal > 0) {
                uchar val = cv::saturate_cast<uchar>(255 - mapVal * 255 / maxVal);
                c[2] = val;
            }
            if (mapVal < 0) {
                uchar val = cv::saturate_cast<uchar>(255 - mapVal * 255 / minVal);
                c[0] = val;
            }
            distMap2.ptr<cv::Vec3b>(i)[j] = c;
        }
    }
    cv::drawContours(distMap2, contours, 0, cv::Scalar::all(255), 1);
    cv::imshow("map", distMap2);
    
    cv::imshow("src", src);
    cv::waitKey(0);
    
    return 0;
}
